/**
 * @file:          SCurve.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.06.20
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.06.20,09:35:01
 */

#include "SCurve.h"
#include "common.h"

uint8 SCurve_Init(SCurve *self)
{
    self->Planning = SCurve_Planning;
    self->Run = SCurve_Run;

    return TRUE;
}

void SCurve_Planning(SCurve *self)
{
    uint32 i = 0;

    if (self->in_pos_target - self->in_pos_origin > 0)
    {
        self->displacement = self->in_pos_target - self->in_pos_origin;
    }
    else
    {
        self->displacement = self->in_pos_origin - self->in_pos_target;
    }
    self->direction = self->in_pos_target - self->in_pos_origin >= 0 ? 1 : -1;

    self->jerk_section[0] = self->param_jerk;
    self->jerk_section[1] = 0;
    self->jerk_section[2] = -self->param_jerk;
    self->jerk_section[3] = 0;
    self->jerk_section[4] = -self->param_jerk;
    self->jerk_section[5] = 0;
    self->jerk_section[6] = self->param_jerk;

    /** Determine SCurve type according to setting condition */
    self->traj_type_condition[0] = self->param_acc_max * self->param_acc_max / self->param_jerk;
    self->traj_type_condition[1] = 2 * pow(self->speed_max, 3 / 2.0) / sqrt(self->param_jerk);
    self->traj_type_condition[2] = self->param_speed_max * (self->param_acc_max * self->param_acc_max + self->param_speed_max * self->param_jerk) / (self->param_jerk * self->param_acc_max);
    self->traj_type_condition[3] = 2 * self->param_acc_max * self->param_acc_max * self->param_acc_max / (self->param_jerk * self->param_jerk);
    if (self->traj_type_condition[0] > self->param_speed_max)
    {
        if (self->displacement >= self->traj_type_condition[1])
        {
            self->traj_type = 2;
        }
        else
        {
            self->traj_type = 1;
        }
    }
    else
    {
        if (self->displacement >= self->traj_type_condition[2])
        {
            self->traj_type = 4;
        }
        else if (self->traj_type_condition[3] <= self->displacement && self->displacement < self->traj_type_condition[2])
        {
            self->traj_type = 3;
        }
        else if (self->displacement < self->traj_type_condition[3])
        {
            self->traj_type = 1;
        }
    }

    /* Calculate the delta T of each section*/
    switch (self->traj_type)
    {
    case 1:
        self->time_delta_s[0] = pow((self->displacement / 2.0 / self->param_jerk), 1 / 3.0);
        self->time_delta_s[1] = 0;
        self->time_delta_s[2] = self->time_delta_s[0];
        self->time_delta_s[3] = 0;
        self->time_delta_s[4] = self->time_delta_s[0];
        self->time_delta_s[5] = 0;
        self->time_delta_s[6] = self->time_delta_s[0];
        break;
    case 2:
        self->time_delta_s[0] = sqrt(self->param_speed_max / self->param_jerk);
        self->time_delta_s[1] = 0;
        self->time_delta_s[2] = self->time_delta_s[0];
        self->time_delta_s[3] = (self->displacement / self->param_speed_max) - 2 * sqrt(self->param_speed_max / self->param_jerk);
        self->time_delta_s[4] = self->time_delta_s[0];
        self->time_delta_s[5] = 0;
        self->time_delta_s[6] = self->time_delta_s[0];
        break;
    case 3:
        self->time_delta_s[0] = self->param_acc_max / self->param_jerk;
        self->time_delta_s[1] = (sqrt(self->param_acc_max * self->param_acc_max * self->param_acc_max + 4 * self->param_jerk * self->param_jerk * self->displacement) - 3 * pow(self->param_acc_max, 1.5)) / (2 * sqrt(self->param_acc_max) * self->param_jerk);
        self->time_delta_s[2] = self->time_delta_s[0];
        self->time_delta_s[3] = 0;
        self->time_delta_s[4] = self->time_delta_s[0];
        self->time_delta_s[5] = self->time_delta_s[1];
        self->time_delta_s[6] = self->time_delta_s[0];
        break;
    case 4:
        self->time_delta_s[0] = self->param_acc_max / self->param_jerk;
        self->time_delta_s[1] = self->param_speed_max / self->param_acc_max - self->param_acc_max / self->param_jerk;
        self->time_delta_s[2] = self->time_delta_s[0];
        self->time_delta_s[3] = (self->displacement / self->param_speed_max) - (self->param_acc_max * self->param_acc_max + self->param_speed_max * self->param_jerk) / (self->param_jerk * self->param_acc_max);
        self->time_delta_s[4] = self->time_delta_s[0];
        self->time_delta_s[5] = self->time_delta_s[1];
        self->time_delta_s[6] = self->time_delta_s[0];
        break;
    default:;
    }

    /** Get timer node each sector switching */
    self->time_node_s[0] = 0.0F;
    for (i = 1; i < 8; i++)
    {
        self->time_node_s[i] = self->time_node_s[i - 1] + self->time_delta_s[i - 1];
    }

    /* Calculate the initial state of each section*/
    self->section_pos_init[0] = self->in_pos_origin;
    self->section_spd_init[0] = 0.0F;
    self->section_acc_init[0] = 0.0F;
    for (i = 1; i < 7; i++)
    {
        self->section_pos_init[i] = self->section_pos_init[i - 1] + self->section_spd_init[i - 1] * self->time_delta_s[i - 1] +
                                    0.5 * self->section_acc_init[i - 1] * pow(self->time_delta_s[i - 1], 2) +
                                    (1.0 / 6.0) * self->jerk_section[i - 1] * pow(self->time_delta_s[i - 1], 3);

        self->section_spd_init[i] = self->section_spd_init[i - 1] + self->section_acc_init[i - 1] * self->time_delta_s[i - 1] +
                                    0.5 * self->jerk_section[i - 1] * pow(self->time_delta_s[i - 1], 2);

        self->section_acc_init[i] = self->section_acc_init[i - 1] + self->jerk_section[i - 1] * self->time_delta_s[i - 1];
    }
}

void SCurve_Run(SCurve *self)
{
    // float self->displacement = 0, self->param_speed_max = 0, self->param_acc_max = 0, self->param_jerk = 0;
    // float self/
    int i = 0;

    if (self->in_run_start == 1)
    {
        self->in_run_start = 0;
        self->out_run_finished = 0;
        self->running = 1;
        self->elapsed_time_count = 0;
        self->elapsed_time_s = 0;
    }

    if (self->running == 1)
    {
        self->current_section = 0;
        /** Get Current Section */
        for (i = 0; i < 7; i++)
        {
            if (self->elapsed_time_s <= self->time_node_s[i + 1])
            {
                self->current_section = i;
                break;
            }
        }

        if (self->elapsed_time_s > self->time_node_s[7])
        {
            self->out_run_finished = 1;
            self->running = 0;
            // /** Current SCurve over */
            // self->UpdataFlag = true;

            // /** All SCurve is finish */
            // if (self->ParameterIndex == SCURVE_NUMBER_MAX)
            // {
            // 	self->EndFlag = true;
            // }
        }
        else
        {
            /** Calculate current section time_node_s cost */
            self->elapsed_time_section_s = self->elapsed_time_s - self->time_node_s[self->current_section];

            /* Set motor rotation direction */
            self->out_pos = self->direction * (self->section_pos_init[self->current_section] + self->section_spd_init[self->current_section] * self->elapsed_time_section_s +
                                               0.5 * self->section_acc_init[self->current_section] * pow(self->elapsed_time_section_s, 2) +
                                               (1.0 / 6.0) * self->jerk_section[self->current_section] * pow(self->elapsed_time_section_s, 3));

            /** Updata Speed vector */
            self->out_spd = self->direction * (self->section_spd_init[self->current_section] + self->section_acc_init[self->current_section] * self->elapsed_time_section_s +
                                               0.5 * self->jerk_section[self->current_section] * pow(self->elapsed_time_section_s, 2));

            /** Updata Acceleration */
            self->out_acc = self->direction * (self->section_acc_init[self->current_section] + self->jerk_section[self->current_section] * self->elapsed_time_section_s);

            /** Recording time_node_s */
            self->elapsed_time_count++;
            self->elapsed_time_s = self->elapsed_time_count * self->param_period_s;

            // DBprintf("%s %s %s\r\n", float2str(self->out_pos, 4), float2str(self->out_spd, 4), float2str(self->out_acc, 4));

        }
    }
}
